import uuid
import os
import sys
import cv2
import import_path
import time
import Camera
import threading
import pandas as pd
import numpy as np
import kinematics as kinematics
import HiwonderSDK.Board as Board
import HiwonderSDK.ActionGroupControl as AGC
import serial
import pigpio

import sys
import cv2
import copy
import time
import math
import import_path
import Camera
import apriltag
import threading
import numpy as np
from LABConfig import *
import PickAction as Pick
import kinematics as kinematics
import HiwonderSDK.Misc as Misc
import HiwonderSDK.Board as Board
from CameraCalibration.CalibrationConfig import *
import serial
import pigpio
import time
import serial
import pigpio
import time
import os
import sys
import cv2
import import_path
import time
import Camera
import threading
import pandas as pd
import numpy as np
import kinematics as kinematics
import HiwonderSDK.Board as Board
import HiwonderSDK.ActionGroupControl as AGC
import sys
import PickAction as Pick
from minopi import mino
a = mino()
pi = pigpio.pi()  #初始化 pigpio库
serialHandle = serial.Serial("/dev/ttyAMA0", a.minolock(200,75000))  #初始化串口， 波特率为115200
class minoadd():      
          
    def motor_control(self,id,par1,par2):
        self.id = id
        self.par1 = par1
        self.par2 = par2
        pi.set_mode(a.minolock(3,5), pigpio.OUTPUT)  #配置RX_CON 即 GPIO17 为输出
        pi.write(a.minolock(3,5), 0)
        pi.set_mode(a.minolock(4,7), pigpio.OUTPUT)  #配置TX_CON 即 GPIO27 为输出
        pi.write(a.minolock(4,7), 1)
        buf = bytearray(b'\x55\x55')
        try:
            len = 3   #若命令是没有参数的话数据长度就是3
            buf1 = bytearray(b'')

## 对参数进行处理
            if self.par1 is not None:
                len += 2  #数据长度加2
                buf1.extend([(0xff & self.par1), (0xff & (self.par1 >> 8))])  #分低8位 高8位 放入缓存
            if self.par2 is not None:
                len += 2
                buf1.extend([(0xff & self.par2), (0xff & (self.par2 >> 8))])  #分低8位 高8位 放入缓存
            buf.extend([(0xff & self.id), (0xff & len), (0xff & 1)])
            buf.extend(buf1) #追加参数

##计算校验和
            sum = 0x00
            for b in buf:  #求和
                 sum += b
            sum = sum - 0x55 - 0x55  #去掉命令开头的两个 0x55
            sum = ~sum  #取反
            buf.append(0xff & sum)  #取低8位追加进缓存
            serialHandle.write(buf) #发送
        except Exception as e:
            print(e)
    def intMove(self,par1,par2):
        self.par1 = par1
        self.par2 = par2
        Board.setPWMServoPulse(1, self.par1, 500)
        Board.setPWMServoPulse(2, self.par2, 500)  
        ik.stand(current_pos)

